﻿using System;
using System.Drawing;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.IO;

using Cudafy;
using Cudafy.Host;
using Cudafy.Translator;

using Emgu.CV;
using Emgu.CV.Structure;

using System.Diagnostics;
using System.Runtime.InteropServices;
using System.ComponentModel;

namespace CUDA
{
    public class CUDAProcessing
    {
        public const int BLOCK_SIZE2D = 32;
        public const int BLOCK_SIZE1D = 1024;
        static CudafyModule km;
        static GPGPU gpu;
        static bool Init = false;
        public static float[] IF = {0};
        

        public static void InitHW()
        {
            km = CudafyTranslator.Cudafy();
            gpu = CudafyHost.GetDevice(CudafyModes.Target);
            gpu.LoadModule(km);
            Init = true;
        }

        public static Bitmap UploadBitmap(Bitmap Bmp1, Bitmap Bmp2,bool AffMReady, Matrix<float> AFFMatrix, bool NormalizationOnRed, double GainShutter, byte bwTrashold,bool colorAdd, byte[] Gamma, double brightness, double Level, DualChannel_v2._0.CustomColor FluorColor,Point C,int CoeffRectSide)
        {
            Rectangle rec = new Rectangle(0, 0, Bmp1.Width, Bmp1.Height);
            int size3 = Bmp1.Width * Bmp1.Height;
            int size = size3 * 3;
            
            
            System.Drawing.Imaging.BitmapData bmpd1 = Bmp1.LockBits(rec, System.Drawing.Imaging.ImageLockMode.ReadWrite, Bmp1.PixelFormat);
            IntPtr bmp1Ptr = bmpd1.Scan0;
            byte[] gbmp1 = gpu.Allocate<byte>(size);
            gpu.CopyToDeviceAsync(bmp1Ptr, 0, gbmp1, 0, size);
            //gpu.Synchronize();
//A generic error occurred in GDI+.
            System.Drawing.Imaging.BitmapData bmpd2 = Bmp2.LockBits(rec, System.Drawing.Imaging.ImageLockMode.ReadWrite, Bmp2.PixelFormat);
            IntPtr bmp2Ptr = bmpd2.Scan0;
            byte[] gbmp2 = gpu.Allocate<byte>(size);
            gpu.CopyToDeviceAsync(bmp2Ptr, 0, gbmp2, 0, size);
            gpu.Synchronize();
            
            byte[] hbmp1 = new byte[size];
            float[,] gAffMatrix = gpu.CopyToDevice(AFFMatrix.Data);
            byte[] ResultBytes = new byte[size];
            //TODO Exception of type 'System.OutOfMemoryException' was thrown.
            float[] BwFloat = new float[size3];
            float[] gBwFloat = gpu.Allocate<float>(size3);
            byte[] gGamma = gpu.CopyToDevice(Gamma);
            float[] gCoef = gpu.CopyToDevice(new float[]{0});

            if (AffMReady)
            {
                gpu.Launch(new dim3(Bmp1.Width / BLOCK_SIZE2D, Bmp1.Height / BLOCK_SIZE2D), new dim3(BLOCK_SIZE2D, BLOCK_SIZE2D)).GoAffineInverse(gbmp1, gAffMatrix, Bmp1.Width, Bmp1.Height);
                gpu.Launch(size3 / BLOCK_SIZE1D, BLOCK_SIZE1D).AffineBW(gbmp1);
            }
            
            if (NormalizationOnRed) gpu.Launch(size3 / BLOCK_SIZE1D, BLOCK_SIZE1D).gBrightnessNormalizationDo(gbmp1, gbmp2, gBwFloat, bwTrashold, GainShutter);
            else gpu.Launch(size3 / BLOCK_SIZE1D, BLOCK_SIZE1D).gBrightnessNormalizationNotDo(gbmp1, gBwFloat, bwTrashold, GainShutter);
            gpu.Synchronize();

            if (NormalizationOnRed) gpu.Launch(new dim3(1, 1), new dim3(CoeffRectSide * 2 + 1, CoeffRectSide * 2 + 1)).CoeffProc(gbmp2, gBwFloat, gCoef, C.X, C.Y, Bmp1.Width);
            else gpu.Launch(new dim3(1, 1), new dim3(CoeffRectSide * 2 + 1, CoeffRectSide * 2 + 1)).CoeffProcNorm(gbmp2, gBwFloat, gCoef, C.X, C.Y, Bmp1.Width);
            gpu.CopyFromDevice(gCoef, IF);

            if (brightness != 1) gpu.Launch(size / BLOCK_SIZE1D, BLOCK_SIZE1D).GammaCorrect(gbmp2, gGamma);

            if (colorAdd) gpu.Launch(size3 / BLOCK_SIZE1D, BLOCK_SIZE1D).ColorAdd(gbmp2, gBwFloat, (float)Level * (float)2.0, FluorColor.R, FluorColor.G, FluorColor.B);
            else gpu.Launch(size3 / BLOCK_SIZE1D, BLOCK_SIZE1D).NoColorAdd(gbmp2, gBwFloat, (float)Level * (float)2.0, FluorColor.R, FluorColor.G, FluorColor.B);
            gpu.Synchronize();
            
            gpu.CopyFromDevice(gbmp2, ResultBytes);
            Marshal.Copy(ResultBytes,0,bmp2Ptr,size);
            
            Bmp1.UnlockBits(bmpd1);
            Bmp2.UnlockBits(bmpd2);
            gpu.FreeAll();
            return Bmp2;
        }
        public static Image<Bgr, Byte> AffineTransform(Image<Bgr, Byte> img, Matrix<float> AFFMatrix)
        {
            if (!Init) InitHW();

            byte[, ,] onechannel = img.Split()[2].Data;
            int size = img.Width * img.Height;
            Image<Bgr, Byte> resimg = img.Clone();

            GCHandle ResPinned = GCHandle.Alloc(resimg.Data, GCHandleType.Pinned);
            IntPtr ResPtr = ResPinned.AddrOfPinnedObject();
            GCHandle ImgPinned = GCHandle.Alloc(onechannel, GCHandleType.Pinned);
            IntPtr ImgPtr = ImgPinned.AddrOfPinnedObject();

            byte[, ,] gImg = gpu.Allocate(onechannel);
            byte[, ,] gAffTransformResult = gpu.Allocate(resimg.Data);
            float[,] gAffMatrix = gpu.CopyToDevice(AFFMatrix.Data);
            gpu.CopyToDeviceAsync(ImgPtr, 0, gImg, 0, size);

            gpu.Launch(new dim3(img.Width / BLOCK_SIZE2D, img.Height / BLOCK_SIZE2D), new dim3(BLOCK_SIZE2D, BLOCK_SIZE2D)).MatMulKernelInverse(gImg, gAffMatrix, gAffTransformResult, img.Width, img.Height);
            gpu.Synchronize();

            gpu.CopyFromDeviceAsync(gAffTransformResult, 0, ResPtr, 0, size * 3);
            ResPtr.Read(resimg.Data);
            gpu.FreeAll();
            ImgPinned.Free();
            ResPinned.Free();
            return (resimg);
        }
        
        [Cudafy]
        private static void MatMulKernel(GThread thread, byte[, ,] A, float[,] B, byte[, ,] C, int CRow, int CCol)
        {
            int blockRow = thread.blockIdx.x * thread.blockDim.x + thread.threadIdx.x;
            int blockCol = thread.blockIdx.y * thread.blockDim.y + thread.threadIdx.y;

            //Affine Transform
            int xx = (int)Math.Round((float)blockRow * B[0, 0] + (float)blockCol * B[0, 1] + B[0, 2]);
            int yy = (int)Math.Round((float)blockRow * B[1, 0] + (float)blockCol * B[1, 1] + B[1, 2]);

            if (xx >= 0 && xx < CRow && yy >= 0 && yy < CCol)
            {
                C[yy, xx, 0] = C[yy, xx, 1] = C[yy, xx, 2] = A[blockCol, blockRow, 0];
            }
            else
            {
                // зачернение "левых" пикселей
                // разница во времени на зачернение 0.6 мс
                xx %= CRow; yy %= CCol;
                if (xx < 0) xx += CRow;
                if (yy < 0) yy += CCol;
                C[yy, xx, 1] = C[yy, xx, 2] = 0; C[yy, xx, 0] = 255;
            }
        }
        [Cudafy]
        private static void MatMulKernelInverse(GThread thread, byte[, ,] A, float[,] B, byte[, ,] C, int CRow, int CCol)
        {
            int blockRow = thread.blockIdx.x * thread.blockDim.x + thread.threadIdx.x;
            int blockCol = thread.blockIdx.y * thread.blockDim.y + thread.threadIdx.y;

            // Inverse Affine Transform
            //int xx = (int)Math.Round((float)blockRow * B[0,0] - (float)blockCol * B[0, 1] - B[0, 2]*B[0,0]+B[1,2]*B[0,1]);
            //int yy = (int)Math.Round(-(float)blockRow * B[1, 0] +(float)blockCol * B[1,1] - B[1, 2]*B[1,1]-B[1,0]*B[0,2]);
            int xx = (int)Math.Round((float)blockRow * B[0, 0] - (float)blockCol * B[0, 1] + B[0, 2]);
            int yy = (int)Math.Round(-(float)blockRow * B[1, 0] + (float)blockCol * B[1, 1] + B[1, 2]);

            if (xx >= 0 && xx < CRow && yy >= 0 && yy < CCol) C[blockCol, blockRow, 0] = C[blockCol, blockRow, 1] = C[blockCol, blockRow, 2] = A[yy, xx, 0];
            else C[blockCol, blockRow, 0] = C[blockCol, blockRow,2] = C[blockCol, blockRow, 1] = 255; 
        }
        [Cudafy]
        private static void GoAffine(GThread thread, byte[] A, float[,] B, int CRow, int CCol)
        {
            int blockRow = thread.blockIdx.x * thread.blockDim.x + thread.threadIdx.x;
            int blockCol = thread.blockIdx.y * thread.blockDim.y + thread.threadIdx.y;
            
            int xx = (int)(blockRow * B[0, 0] + blockCol * B[0, 1] + B[0, 2]);
            int yy = (int)(blockRow * B[1, 0] + blockCol * B[1, 1] + B[1, 2]);
            int lin = 3*(xx  + yy* CRow);

            if (xx >= 0 && xx < CRow && yy >= 0 && yy < CCol)
            {
                A[lin + 1] = A[(blockRow + blockCol * CRow) * 3];
            }
            else
            {
                xx %= CRow; yy %= CCol;
                if (xx < 0) xx += CRow;
                if (yy < 0) yy += CCol;
                lin = 3 * (xx + yy * CRow);
                A[lin + 1] = 0;
            }
        }
        [Cudafy]
        private static void GoAffineInverse(GThread thread, byte[] A, float[,] B, int CRow, int CCol)
        {
            int blockRow = thread.blockIdx.x * thread.blockDim.x + thread.threadIdx.x;
            int blockCol = thread.blockIdx.y * thread.blockDim.y + thread.threadIdx.y;

            int xx = (int)((float)blockRow * B[0, 0] - (float)blockCol * B[0, 1] + B[0, 2]);
            int yy = (int)(-(float)blockRow* B[1, 0] + (float)blockCol * B[1, 1] + B[1, 2]);

            if (xx >= 0 && xx < CRow && yy >= 0 && yy < CCol) A[(blockRow + blockCol * CRow) * 3 + 1] = A[3 * (xx + yy * CRow)];
            else                                              A[(blockRow + blockCol * CRow) * 3 + 1] = 0;
        }
        [Cudafy]
        private static void AffineBW(GThread thread, byte[] A)
        {
            int id = (thread.blockIdx.x * thread.blockDim.x + thread.threadIdx.x) * 3;
            A[id] = A[id + 2] = A[id+1];
        }
        [Cudafy]
        private static void gBrightnessNormalizationDo(GThread thread, byte[] bwBytesPtr,byte[] redBytesPtr , float[] bwFloatPtr, byte bwTrashold, double GainShutter)
        {
            int id = thread.blockIdx.x * thread.blockDim.x + thread.threadIdx.x;
            int idR = id * 3 + 2;

            //bwBytesPtr[idR] = (byte)((redBytesPtr[idR] + bwBytesPtr[idR]) / 2);
            //bwBytesPtr[idR + 1] = (byte)((redBytesPtr[idR + 1] + bwBytesPtr[idR + 1]) / 2);
            //bwBytesPtr[idR + 2] = (byte)((redBytesPtr[idR + 2] + bwBytesPtr[idR + 2]) / 2);
            if (redBytesPtr[idR] < 10 || redBytesPtr[idR] > 254 || bwBytesPtr[idR] < bwTrashold)
            {
                bwFloatPtr[id] = 0;
                //redBytesPtr[idR] = 255;
                //bwBytesPtr[idR] = 0;
            }
            else bwFloatPtr[id] = (float)bwBytesPtr[idR]/ (float)redBytesPtr[idR] * (float)GainShutter;
        }
        [Cudafy]
        private static void gBrightnessNormalizationNotDo(GThread thread, byte[] bwBytesPtr, float[] bwFloatPtr, byte bwTrashold, double GainShutter)
        {
            int id = thread.blockIdx.x * thread.blockDim.x + thread.threadIdx.x;
            int idR = id * 3;

            if (bwBytesPtr[idR] < bwTrashold) { /* bwBytesPtr[idR] = 0;*/ bwFloatPtr[id] = 0; }
            else bwFloatPtr[id] = (float)((float)bwBytesPtr[idR] / 127.0f * (float)GainShutter);
        }

        [Cudafy]
        private static void GammaAdduction(GThread thread, byte[] bmp2, byte[] Gamma)
        {
            int id = (thread.blockIdx.x * thread.blockDim.x + thread.threadIdx.x);
            bmp2[id] = Gamma[bmp2[id]];
        }
        [Cudafy]
        private static void CoeffProc(GThread thread, byte[] bmp2, float[] BwFloat, float[] Coef, int xc, int yc, int W)
        {
            int idx = thread.blockIdx.x * thread.blockDim.x + thread.threadIdx.x + xc;
            int idy = thread.blockIdx.y * thread.blockDim.y + thread.threadIdx.y + yc;
            int k = (idy * W + idx) * 3;
            Coef[0] += BwFloat[k / 3];
            bmp2[k] = (byte)(bmp2[k] * 0.5);
            bmp2[k + 1] = (byte)(bmp2[k + 1] * 0.5);
            bmp2[k + 2] = (byte)(bmp2[k + 2] * 0.5);
        }
        [Cudafy]
        private static void CoeffProcNorm(GThread thread, byte[] bmp2, float[] BwFloat, float[] Coef, int xc, int yc, int W)
        {
            int idx = thread.blockIdx.x * thread.blockDim.x + thread.threadIdx.x + xc;
            int idy = thread.blockIdx.y * thread.blockDim.y + thread.threadIdx.y + yc;
            int k = (idy * W + idx) * 3;
            if (bmp2[k + 2] > 10 || bmp2[k + 2] < 254)
            Coef[0] += BwFloat[k / 3] * 127.0f / bmp2[k+2];
            bmp2[k] = (byte)(bmp2[k] * 0.5);
            bmp2[k + 1] = (byte)(bmp2[k + 1] * 0.5);
            bmp2[k + 2] = (byte)(bmp2[k + 2] * 0.5);
        }

        [Cudafy]
        private static void GammaCorrect(GThread thread,byte[] bmp2, byte[] Gamma)
        {
            int id = (thread.blockIdx.x * thread.blockDim.x + thread.threadIdx.x);
            bmp2[id] = Gamma[bmp2[id]];
        }
        [Cudafy]
        private static void ColorAdd(GThread thread, byte[] bmp2, float[] BwFloat, float level, float FluorRed, float FluorGreen, float FluorBlue)
        {
            int id = (thread.blockIdx.x * thread.blockDim.x + thread.threadIdx.x)*3;
            float TempColor;

            float tempBwFloat = level*(float)BwFloat[id/3];
            if (tempBwFloat >= 255) tempBwFloat = 254;
            if (tempBwFloat < 2) tempBwFloat = 0;

            TempColor = (float)bmp2[id] + tempBwFloat * FluorBlue;
            if (TempColor > 255) TempColor = 255;
            bmp2[id] = (byte)TempColor;
            id++;

            TempColor = (float)bmp2[id] + tempBwFloat * FluorGreen;
            if (TempColor > 255) TempColor = 255;
            bmp2[id] = (byte)TempColor;
            id++;

            TempColor = (float)bmp2[id] + tempBwFloat * FluorRed;
            if (TempColor > 255) TempColor = 255;
            bmp2[id] = (byte)TempColor;
        }
        [Cudafy]
        private static void NoColorAdd(GThread thread, byte[] bmp2, float[] BwFloat, float level, float FluorRed, float FluorGreen, float FluorBlue)
        {
            int id = (thread.blockIdx.x * thread.blockDim.x + thread.threadIdx.x) * 3;
            float TempColor;

            float tempBwFloat = level * BwFloat[id / 3];
            if (tempBwFloat >= 255) tempBwFloat = 254;
            if (tempBwFloat < 2) tempBwFloat = 0;

            TempColor = (float)bmp2[id] + tempBwFloat * (FluorBlue - 0.5f);
            if (TempColor > 255) TempColor = 255;
            if (TempColor < 0) TempColor = 0;
            bmp2[id] = (byte)TempColor;
            id++;

            TempColor = (float)bmp2[id] + tempBwFloat * (FluorGreen - 0.5f);
            if (TempColor > 255) TempColor = 255;
            if (TempColor < 0) TempColor = 0;
            bmp2[id] = (byte)TempColor;
            id++;

            TempColor = (float)bmp2[id] + tempBwFloat * (FluorRed - 0.5f);
            if (TempColor > 255) TempColor = 255;
            if (TempColor < 0) TempColor = 0;
            bmp2[id] = (byte)TempColor;
        }
    }
}
